/*
 * @Author: aggfb 714740235@qq.com
 * @Date: 2024-10-01 10:22:46
 * @LastEditors: aggfb 714740235@qq.com
 * @LastEditTime: 2024-10-01 16:44:52
 * @FilePath: \baking_machine\bsp\bap_timer.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "foc.h"

/**
 * @brief clark
 * @param None
 * @retval None
 * @note ialpha = ia
 *       ibeta = (ia+2ib)/sqrt(3)
 */
void clark(foc_t *foc)
{
    foc->ialpha = foc->ia;
    foc->ibeta = (foc->ia * ONE_BY_SQRT3) + (foc->ib * 2 * ONE_BY_SQRT3);
}

/**
 * @brief park
 * @param None
 * @retval None
 * @note Id = ialpha*cos(theta) + ibeta*sin(theta)
 *       Iq = -ialpha*sin(theta) + ibeta*cos(theta)
 */
void park(foc_t *foc)
{
    foc->id = (foc->ialpha * foc->cos) + (foc->ibeta * foc->sin);
    foc->iq = (foc->ibeta * foc->cos) - (foc->ialpha * foc->sin);
}

/**
 * @brief inv_park
 * @param None
 * @retval None
 * @note valpha = vd*cos(theta) - vq*sin(theta)
 *       vbeta  = vd*sin(theta) + vq*cos(theta)
 */
void inv_park(foc_t *foc)
{
    foc->valpha = (foc->vd * foc->cos) - (foc->vq * foc->sin);
    foc->vbeta = (foc->vd * foc->sin) + (foc->vq * foc->cos);
}

/**
 * @brief sin cos calculate
 * @param None
 * @retval None
 * @note
 */
void sin_cos(foc_t *foc)
{
    foc->sin = arm_sin_f32(foc->angle);
    foc->cos = arm_cos_f32(foc->angle);
}

/**
 * @brief
 * @param None
 * @retval None
 * @note for one shunt,Prevent duty cycles from being equal,tb must be increased by 1
 */
void svpwm(foc_t *foc)
{
    float vr1 = 0, vr2 = 0, vr3 = 0;
    float t1w = 0, t2w = 0;
    float t1 = 0, t2 = 0, t3 = 0;

    vr1 = foc->vbeta;
    vr2 = (-foc->vbeta / 2 + SQRT3_BY2 * foc->valpha);
    vr3 = (-foc->vbeta / 2 - SQRT3_BY2 * foc->valpha);

    if (vr3 > 0)
    {
        if (vr2 > 0)
        {
            foc->sector = 5;

            t1w = foc->pwm_period * vr2;
            t2w = foc->pwm_period * vr3;

            t1 = (foc->pwm_period - t1w - t2w) / 2;
            t2 = t1 + t1w;
            t3 = t2 + t2w;

            foc->pwma = t2;
            foc->pwmb = t1;
            foc->pwmc = t3;
        }
        else
        {
            if (vr1 > 0)
            {
                foc->sector = 3;

                t1w = foc->pwm_period * vr3;
                t2w = foc->pwm_period * vr1;

                t1 = (foc->pwm_period - t1w - t2w) / 2;
                t2 = t1 + t1w;
                t3 = t2 + t2w;

                foc->pwma = t1;
                foc->pwmb = t3;
                foc->pwmc = t2;
            }
            else
            {
                foc->sector = 4;

                t1w = foc->pwm_period * -vr2;
                t2w = foc->pwm_period * -vr1;

                t1 = (foc->pwm_period - t1w - t2w) / 2;
                t2 = t1 + t1w;
                t3 = t2 + t2w;

                foc->pwma = t1;
                foc->pwmb = t2;
                foc->pwmc = t3;
            }
        }
    }
    else
    {
        if (vr2 > 0)
        {
            if (vr1 > 0)
            {
                foc->sector = 1;

                t1w = foc->pwm_period * vr1;
                t2w = foc->pwm_period * vr2;

                t1 = (foc->pwm_period - t1w - t2w) / 2;
                t2 = t1 + t1w;
                t3 = t2 + t2w;

                foc->pwma = t3;
                foc->pwmb = t2;
                foc->pwmc = t1;
            }
            else
            {
                foc->sector = 6;

                t1w = foc->pwm_period * -vr1;
                t2w = foc->pwm_period * -vr3;

                t1 = (foc->pwm_period - t1w - t2w) / 2;
                t2 = t1 + t1w;
                t3 = t2 + t2w;

                foc->pwma = t3;
                foc->pwmb = t1;
                foc->pwmc = t2;
            }
        }
        else
        {
            foc->sector = 2;

            t1w = foc->pwm_period * -vr3;
            t2w = foc->pwm_period * -vr2;

            t1 = (foc->pwm_period - t1w - t2w) / 2;
            t2 = t1 + t1w;
            t3 = t2 + t2w;

            foc->pwma = t2;
            foc->pwmb = t3;
            foc->pwmc = t1;
        }
    }
}
